
#include "user.h"
#include <stdio.h>
#include <stdlib.h>

#include "string.h"

#define VERSION(COM) ">>> Usart: "#COM"\r\n"
#define APP_VERSION "Mavlink RC EXTRA--V1.0  <By Awesome>\r\n"

const uint32_t BAUDRATE[] = { 110, 300, 600, 1200, 1800, 2400, 3600, 4800, 7200, 9600, \
                              14400, 19200, 28800, 38400, 57600, 115200, 12800, \
															230400, 256000, 460800, 500000, 512000, 600000, 750000,\
															921600, 1000000};

mavlink_system_t gcs_mavlink_system = {255, 1};

CameraConfig camera_config;
MotorConfig  motor_config;
RCMap rc_map;
uint32_t usart2_baudrate;
uint32_t usart3_baudrate;												

//led define
uint32_t led_blink_time = 1000;
uint32_t led_callback_time = 0;

//usart define
#define USART_NUM 3

awesome_usart usarts[USART_NUM];

#define USART1_RX_BUFF_SIZE 1024
#define USART1_TX_BUFF_SIZE 1024
#define USART1_RX_TMP_SIZE  512
#define USART1_TX_TMP_SIZE  512 

#define USART2_RX_BUFF_SIZE 1024
#define USART2_TX_BUFF_SIZE 1024
#define USART2_RX_TMP_SIZE  512
#define USART2_TX_TMP_SIZE  512 

#define USART3_RX_BUFF_SIZE 1024
#define USART3_TX_BUFF_SIZE 1024
#define USART3_RX_TMP_SIZE  512
#define USART3_TX_TMP_SIZE  512 

uint32_t usart_process_time = 0;
uint8_t  usart_tx_comp[USART_NUM] = { 0, 0, 0 };   //0-没有数据发送，1-数据发送完成，2-数据发送中

#define config_usart usarts[1]

#if USART1_ENABLE==1
	uint8_t  rx1buff[USART1_RX_BUFF_SIZE];
	uint8_t  tx1buff[USART1_TX_BUFF_SIZE];
	uint8_t  tx1_tmp_buff[USART1_TX_TMP_SIZE];
	uint8_t  rx1_tmp_buff[USART1_RX_TMP_SIZE];
	uint8_t  it1[2];
#endif

#if USART2_ENABLE==1
	uint8_t  rx2buff[USART2_RX_BUFF_SIZE];
	uint8_t  tx2buff[USART2_TX_BUFF_SIZE];
	uint8_t  tx2_tmp_buff[USART2_TX_TMP_SIZE];
	uint8_t  rx2_tmp_buff[USART2_RX_TMP_SIZE];
	uint8_t  it2[2];
#endif

#if USART3_ENABLE==1
	uint8_t  rx3buff[USART3_RX_BUFF_SIZE];
	uint8_t  tx3buff[USART3_TX_BUFF_SIZE];
	uint8_t  tx3_tmp_buff[USART3_TX_TMP_SIZE];
	uint8_t  rx3_tmp_buff[USART3_RX_TMP_SIZE];
	uint8_t  it3[2];
#endif
 
//at config
AT at;
AT_PARSE_STATUS at_status;

#define MAX_PARAM_LEN		18
uint32_t params[MAX_PARAM_LEN];
uint32_t reset_params[MAX_PARAM_LEN];
uint32_t old_params[MAX_PARAM_LEN];

//消息字符串
unsigned char msg_string[128];

//其他控制变量定义
uint32_t mavlink_request_send_time = 0;     //30s发送一次
uint32_t camera_open_time = 0;
uint32_t camera_close_time = 0;
uint32_t camera_take_photo_time = 0;
uint32_t camera_record_time = 0;

uint8_t  camera_open_close_flag = 0;
uint8_t  camera_take_photo_flag = 0;
uint8_t  motor_open_close_flag = 0;
uint32_t camera_record_flag = 0;

uint16_t camera_open_close_old_pwm = 0;
uint16_t camera_record_old_pwm = 0;
uint16_t camera_take_photo_old_pwm = 0;
uint16_t motor_open_close_old_pwm  = 0;

uint32_t rc_last_update_time = 0;
uint32_t rc_send_callback_time = 0;
uint8_t  have_rc = 0;

uint32_t usart2_update_time = 0;
uint32_t usart3_update_time = 0;

uint32_t iwdg_time = 0;
uint8_t load_param_error = 0;

static void usart_run(void);
static void set_usart(UART_HandleTypeDef* pu, uint32_t bd);
static void reconfig_usart(uint8_t id);
static void at_process(uint8_t data);
static uint8_t is_baudrate(uint32_t bd);
static void init_config( void );
static void init_param(void);
static void set_param(void);
static uint8_t parse_camera_config(const char* src);
static uint8_t parse_motor_config(const char* src);
static uint8_t parse_rc_map_config(const char* src);
static void get_param_msg(void);
static void mavlink_request_data(void);
static void camera_open(uint8_t on);
static void camera_photo(uint8_t on);
static void rc_process(mavlink_rc_channels_t* rc);
static void debug_string(const uint8_t* str);
static void set_old_param(void);
static void set_param_from_old(void);
static void reset_param(void);
static void restart_usart_check(void);
static void init_gpio_level(void);
static void update_motor_output(void);
static void camera_record(uint8_t on);

void system_init(void){	
	
	init_config();
	init_param();
		
	if(load_param(reset_params, params, MAX_PARAM_LEN)==0) load_param_error=1;
	else set_param();
	
	/* 保存参数失败，使用默认配置 */
	if(load_param_error){
		init_config();
		init_param();
	}
	
	init_gpio_level();
			
	led_off();
	
#if USART1_ENABLE==1
	reconfig_usart(1);   //重新配置串口
	usart_init(&usarts[0], rx1buff, USART1_RX_BUFF_SIZE, tx1buff, USART1_TX_BUFF_SIZE);
	usarts[0].writes(&usarts[0], VERSION(COM01), strlen(VERSION(COM01)));
#endif

#if USART2_ENABLE==1	
  reconfig_usart(2);
	usart_init(&usarts[1], rx2buff, USART2_RX_BUFF_SIZE, tx2buff, USART2_TX_BUFF_SIZE);
	usarts[1].writes(&usarts[1], VERSION(COM02), strlen(VERSION(COM02)));
#endif

#if USART3_ENABLE==1	
  reconfig_usart(3);
	usart_init(&usarts[2], rx3buff, USART3_RX_BUFF_SIZE, tx3buff, USART3_TX_BUFF_SIZE);
	usarts[2].writes(&usarts[2], VERSION(COM03), strlen(VERSION(COM03)));
#endif
	
	sprintf((char*)msg_string, "UART2: %d, UART3: %d \r\n", usart2_baudrate, usart3_baudrate);
	HAL_UART_Transmit(&huart2, msg_string, strlen((const char*)msg_string), 100);
	
	mavlink_request_data();
		
	//开启串口中断
#if USART1_ENABLE==1
	start_usart_rx(&huart1);
#endif

#if USART2_ENABLE==1	
	start_usart_rx(&huart2);	
#endif

#if USART3_ENABLE==1	
	start_usart_rx(&huart3);	
#endif

	update_motor_output();
	
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
}

void app(void){
	//100ms喂狗
	if( HAL_GetTick()-iwdg_time>=80 ){
		HAL_IWDG_Refresh(&hiwdg);
		iwdg_time = HAL_GetTick();
	}
		
	//led run
	if( HAL_GetTick()-led_callback_time>=10 ){			
		if( have_rc ){
			led_blink_time = 100;
		}else{
			if(load_param_error){
				led_blink_time = 5000;   		//加载参数时发生错误
			}else led_blink_time = 1000;
		}
		led_run(led_blink_time);
		led_callback_time = HAL_GetTick();
	}
	
  //usart run	
	if( HAL_GetTick()-usart_process_time>=5 ){
		usart_run();
		usart_process_time = HAL_GetTick();
	}
	
	//send mavlink request send data
	if( HAL_GetTick()-mavlink_request_send_time>=30*1000ul ){
		mavlink_request_data();
		mavlink_request_send_time = HAL_GetTick();
	}
	
	//camera control open / close / take photo
	if( 1==camera_open_close_flag ){  			//1 == close
		camera_open(1);
		if( HAL_GetTick()-camera_open_time>=camera_config.open_time_ms ){
			camera_open(0);
			camera_open_close_flag = 0;
		}
	}
	
	//take photo
	if( 1==camera_take_photo_flag ){
		camera_photo(1);
		if( HAL_GetTick()-camera_take_photo_time>=camera_config.take_photo_time_ms ){
			camera_photo(0);
			camera_take_photo_flag = 0;
		}
	}

	//record
	if( 1==camera_record_flag ){
		camera_record(1);
		if( HAL_GetTick()-camera_record_time>=camera_config.record_time_ms ){
			camera_record(0);
			camera_record_flag = 0;
		}
	}
	
	//motor control open and close
	if( 1==motor_open_close_flag ){   //open
		htim1.Instance->CCR2 = motor_config.open_pwm;
		htim1.Instance->CCR3 = motor_config.open_pwm;
	}else if( 2==motor_open_close_flag ){ //close
		htim1.Instance->CCR2 = motor_config.close_pwm;
		htim1.Instance->CCR3 = motor_config.close_pwm;
	}
	
	restart_usart_check();   //如果1min中内没有接受数据，则重新启动串口
}

static void usart_run(void){
	
	uint8_t i = 0;
	uint32_t len = 0;
	uint32_t buff_len_t = 0, buff_len_r = 0;
	uint8_t *pbt=0, *pbr=0;
	UART_HandleTypeDef* pu = 0;
	uint8_t id;   /* 1-usart1  2-usart2 */
	mavlink_message_t mavlink_msg;
	mavlink_status_t  mavlink_sta;
	mavlink_rc_channels_t rc_raw;
	
  //send data process
	for( i=0; i<USART_NUM; ++i ){
		if(0==i){
			#if USART1_ENABLE==1
				pbt = tx1_tmp_buff;
				pbr = rx1_tmp_buff;
				buff_len_t = USART1_TX_TMP_SIZE;
				buff_len_r = USART1_RX_TMP_SIZE;
				pu = &huart1;
				id = 1;
			#endif
		}else if(1==i){
			#if USART2_ENABLE==1
				pbt = tx2_tmp_buff;
				pbr = rx2_tmp_buff;
				buff_len_t = USART2_TX_TMP_SIZE;
				buff_len_r = USART2_RX_TMP_SIZE;
				pu = &huart2;
				id = 2;
			#endif
		}else if(2==i){
			#if USART3_ENABLE==1
				pbt = tx3_tmp_buff;
				pbr = rx3_tmp_buff;
				buff_len_t = USART3_TX_TMP_SIZE;
				buff_len_r = USART3_RX_TMP_SIZE;
				pu = &huart3;
				id = 3;
			#endif
		}
		
		if( pu!=0 && usart_tx_comp[i]!=2 ){			
			//send data
			len = usarts[i].low_get_write_buff(&usarts[i], pbt, buff_len_t);
			if(len>0){
				HAL_UART_Transmit_DMA(pu, pbt, len);
				usart_tx_comp[i] = 2;
			}
		}	
		
		//config and debug serial
		if(id==2){
			len = usarts[i].reads(&usarts[i], pbr, buff_len_r);
			if(len>0){
				uint32_t i = 0;
				for( i=0; i<len; ++i ){
					at_process(pbr[i]);					
				}
			}
		}
		
		else if(id==3){   //mavlink
			len = usarts[i].reads(&usarts[i], pbr, buff_len_r);
			if(len>0){
				uint32_t i = 0;
				for( i=0; i<len; ++i ){
					if(MAVLINK_FRAMING_OK==mavlink_parse_char(MAVLINK_COMM_3, pbr[i], &mavlink_msg, &mavlink_sta)){
						//解析数据完成
						if( mavlink_msg.msgid==MAVLINK_MSG_ID_RC_CHANNELS ){
							//debug_string("Have RC\r\n");
							have_rc = 1;
							mavlink_msg_rc_channels_decode(&mavlink_msg, &rc_raw);	
							rc_process(&rc_raw);
						}					
					}				
				}
			}
		}		
	}
	
	if(HAL_GetTick()-rc_last_update_time>5000){
		have_rc = 0;
		rc_last_update_time = HAL_GetTick();
		debug_string("\r\n None RC Input!\r\n");
	}
}

void led_run(uint32_t time_ms){
	static uint32_t count = 0; 
	
	uint32_t c = time_ms/10;  //10ms
	
	++count;
	
	if(count<c){
		led_off();
	}else if(count<2*c){
		led_on();
	}else if(count>=2*c){
		count = 0;
	}
}

//打开串口接收中断
void start_usart_rx(UART_HandleTypeDef *huart){
	#if USART1_ENABLE==1
		if(huart==&huart1){
			 HAL_UART_Receive_IT(&huart1, it1, 2);
		}
	#endif
	
	#if USART2_ENABLE==1
		if(huart==&huart2){
			 HAL_UART_Receive_IT(&huart2, it2, 2);
		}
  #endif
	
	#if USART3_ENABLE==1
		if(huart==&huart3){
			 HAL_UART_Receive_IT(&huart3, it3, 2);
		}
	#endif
}

//设置从串口读取的数据
void usart_set_read_buff(UART_HandleTypeDef *huart, uint8_t data){
	#if USART1_ENABLE==1
		if(huart==&huart1){
			 usarts[0].low_set_read_buff(&usarts[0], data);
		}
	#endif
	
	#if USART2_ENABLE==1
		if(huart==&huart2){
			 usarts[1].low_set_read_buff(&usarts[1], data);
			 usart2_update_time = HAL_GetTick();
		}
	#endif
	
	#if USART3_ENABLE==1
		if(huart==&huart3){
			 usarts[2].low_set_read_buff(&usarts[2], data);
			usart3_update_time = HAL_GetTick();
		}
	#endif
	
}

//串口发送中断完成，置发送成功位
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	#if USART1_ENABLE==1
		if(huart==&huart1){
			 usart_tx_comp[0] = 1;
		}
	#endif
	
	#if USART2_ENABLE==1
		if(huart==&huart2){
			 usart_tx_comp[1] = 1;
			 usart2_update_time = HAL_GetTick();
		}
	#endif
		
	#if USART3_ENABLE==1
		if(huart==&huart3){
			 usart_tx_comp[2] = 1;
			 usart3_update_time = HAL_GetTick();
		}
	#endif
}

static void reconfig_usart(uint8_t id){
		
	if(3==id && is_baudrate(usart3_baudrate)){
		//reconfig usart1
		set_usart(&huart3, usart3_baudrate);
	}
	if(2==id && is_baudrate(usart2_baudrate)){
		set_usart(&huart2, usart2_baudrate);
	}
		
}

static void set_usart(UART_HandleTypeDef* pu, uint32_t bd){
	uint8_t i = 0;
	if( pu==&huart3 ) i=2;
	else if( pu==&huart2 ) i=1;
	//else if( pu==&huart2 ) i=1;
	HAL_UART_Abort_IT(pu);
	HAL_UART_DeInit(pu);
	pu->Init.BaudRate = bd;
	HAL_UART_Init(pu);
	start_usart_rx(pu);
	usart_tx_comp[i] = 1;
}

static void at_process(uint8_t data){
	if( AT_PARSE_STATUS_END==at_parse(data, &at, &at_status) ){
		char type[10];
		char datas[50];
		char msg[20];
		uint32_t num = 0;
		uint8_t num_flag = 0;
		uint8_t result = 0;
		AT_TYPE at_type = AT_TYPE_NONE;
		if( at_have_header(&at) ){
			if( at_get_type(&at, type)!=(void*)0 ){
				strtolwr(type);
				if(strcmp(type, "usart2")==0){
					num_flag = 1;
					at_type = AT_TYPE_SERIAL2;
					result = 1;
				}
				
				else if(strcmp(type, "usart3")==0){
					num_flag = 1;
					at_type = AT_TYPE_SERIAL3;
					result = 1;
				}
				
				else if(strcmp(type, "camera")==0){
					if(at_get_data(&at, datas)!=(void*)0){
						if(parse_camera_config(datas)) result = 1;
					}
				}
				
				else if(strcmp(type, "motor")==0){
					if(at_get_data(&at, datas)!=(void*)0){
						if(parse_motor_config(datas)) result = 1;
					}
				}
				
				else if(strcmp(type, "rcmap")==0){
					if(at_get_data(&at, datas)!=(void*)0){
						if(parse_rc_map_config(datas)) result = 1;
					}
				}
				
				else if(strcmp(type, "save")==0){
          set_param();
					if( 0!=save_param(params, MAX_PARAM_LEN) ){
						result = 1;
					} 
				}
				
				else if(strcmp(type, "reset")==0){
					reset_param();
					set_param();   //参数生效
					reconfig_usart(2);
					reconfig_usart(3);
					result = 1;
				}
				
				else if(strcmp(type, "param")==0){
					get_param_msg();
					result = 1;
				}
				
				else if(strcmp(type, "ver")==0){
					usarts[1].writes(&usarts[1], APP_VERSION, strlen((const char*)APP_VERSION));
					result = 1;
				}				
				
				if( num_flag && at_get_data(&at, datas)!=(void*)0 ){
					if( is_num(datas) ){
						num = (uint32_t)atoi(datas);
					}
			 }
			
			 switch(at_type){
				 case AT_TYPE_SERIAL3:
					 if( is_baudrate(num) ){
						 usart3_baudrate = num;
						 init_param();
						 reconfig_usart(3);
					 }else result = 0;
					 break;
				 case AT_TYPE_SERIAL2:
					 if( is_baudrate(num) ){
						 usart2_baudrate = num;
						 init_param();
						 reconfig_usart(2);
					 }else result = 0;
					 break;
				 default:
					 break;
			 }	
			 if(result){
				  set_param();
				  sprintf(msg, "\r\n$%s,OK\r\n", type);
			 }else {
					sprintf(msg, "\r\n$%s,ERROR\r\n", type);
			 }
			 usarts[1].writes(&usarts[1], (uint8_t*)msg, strlen(msg));			 
			}
			
		}
	}
}

static uint8_t is_baudrate(uint32_t bd){
	uint32_t i = 0;
	for( i=0; i<sizeof(BAUDRATE)/4; ++i ){
		if( bd==BAUDRATE[i] ) return 1;
	}
	return 0;
}

static void init_config( void ){
	camera_config.open_close_level_in = 0;
	camera_config.open_level = 0;
	camera_config.record_level = 0;
	camera_config.open_time_ms = 500;
	camera_config.record_time_ms = 500;
	camera_config.take_photo_level = 0;
	camera_config.take_photo_time_ms = 500;

	motor_config.rc_error = 50;
	motor_config.rc_min_pwm = 800;
	motor_config.rc_max_pwm = 2200;
	motor_config.open_pwm = 1600;
	motor_config.rc_trim_pwm = (2200-800)/2 + 800;
	motor_config.close_pwm = 1000;
	
	usart3_baudrate = 115200;
	usart2_baudrate = 115200;
	
  rc_map.camera_open_close_map = 7;
	rc_map.camera_take_photo_map = 8;
	rc_map.camera_record_map = 9;
	rc_map.motor_map = 10;
}

static void init_param(void) {
	uint8_t i = 0;
	
	params[0] = 0x0f & ((camera_config.open_close_level_in<<3) | (camera_config.open_level<<2) | (camera_config.record_level<<1) | (camera_config.take_photo_level<<0));
	params[1] = camera_config.open_time_ms;
	params[2] = camera_config.record_time_ms;
	params[3] = camera_config.take_photo_time_ms;
	
	params[4] = motor_config.rc_min_pwm;
  params[5] = motor_config.rc_trim_pwm;
	params[6] = motor_config.rc_max_pwm;
	params[7] = motor_config.rc_error;
	params[8] = motor_config.open_pwm;
	params[9] = motor_config.close_pwm;
	
	params[10] = usart2_baudrate;
	params[11] = usart3_baudrate;	
	
	params[12] = rc_map.camera_open_close_map;
	params[13] = rc_map.camera_take_photo_map;
	params[14] = rc_map.camera_record_map;
	params[15] = rc_map.motor_map;
	
	for( i=0; i<MAX_PARAM_LEN; ++i ) {
		reset_params[i] = params[i];
	}
}


static void set_param(void){
	camera_config.open_close_level_in = (uint8_t)((params[0]>>3)&0x01);
	camera_config.open_level 					= (uint8_t)((params[0]>>2)&0x01);
	camera_config.record_level				= (uint8_t)((params[0]>>1)&0x01);
	camera_config.take_photo_level 		= (uint8_t)((params[0]>>0)&0x01);
	
	camera_config.open_time_ms	=	params[1];
	camera_config.record_time_ms	=	params[2]; 
	camera_config.take_photo_time_ms	=	params[3];
	
	motor_config.rc_min_pwm		=		params[4];
	motor_config.rc_trim_pwm	=		params[5];
	motor_config.rc_max_pwm		=		params[6];
	motor_config.rc_error     =   params[7];
	motor_config.open_pwm			=		params[8];
	motor_config.close_pwm		=		params[9];
	
	usart2_baudrate						=		params[10];
	usart3_baudrate						=		params[11];		
	
  rc_map.camera_open_close_map 	= params[12];
	rc_map.camera_take_photo_map 	= params[13];
	rc_map.camera_record_map 			= params[14];
	rc_map.motor_map 							= params[15];
	
  init_gpio_level();
	
	update_motor_output();
}

static uint8_t parse_camera_config(const char* src){
	uint8_t offset = 0;
	const char* p = src;
	char buff[16] = {0};
	char* d = buff;
	uint8_t count = 0;
	
	set_old_param();
	
	while( *p!='\0' ){
		if( *p==',' ){
			*d = '\0';
			
			if( !is_num(buff) ) 
			{
				set_param_from_old();
				return 0;
			}
			
			if( 0==offset ){
				params[0] = 0;
				params[0] = (((uint32_t)atoi(buff))&0x01)<<3;
			}else if( 1==offset ){
				params[0] += (((uint32_t)atoi(buff))&0x01)<<2;
			}else if( 2==offset ){
				params[0] += (((uint32_t)atoi(buff))&0x01)<<1;
			}else if( 3==offset ){
				params[0] += (((uint32_t)atoi(buff))&0x01)<<0;
			}else if( 4==offset ){
				params[1] = (uint32_t)atoi(buff);
			}else if( 5==offset ){
				params[2] = (uint32_t)atoi(buff);
			}else if( 6==offset ){
				params[3] = (uint32_t)atoi(buff);
			}
					
			++offset;
			d = buff;
			++p;
			++count;
			continue;
		}
		*d = *p;
		++d;
		++p;
	}
	
		
	*d = '\0';			
	if( !is_num(buff) ) 
	{
		set_param_from_old();
		return 0;
	}

	if( 0==offset ){
		params[0] = 0;
		params[0] += (((uint32_t)atoi(buff))&0x01)<<3;
	}else if( 1==offset ){
		params[0] += (((uint32_t)atoi(buff))&0x01)<<2;
	}else if( 2==offset ){
		params[0] += (((uint32_t)atoi(buff))&0x01)<<1;
	}else if( 3==offset ){
		params[0] += (((uint32_t)atoi(buff))&0x01)<<0;
	}else if( 4==offset ){
		params[1] = (uint32_t)atoi(buff);
	}else if( 5==offset ){
		params[2] = (uint32_t)atoi(buff);
	}else if( 6==offset ){
		params[3] = (uint32_t)atoi(buff);
	}
	++count;
	
	if(count!=7){
		set_param_from_old();
		return 0;
	}
	
	return 1;
}

static uint8_t parse_motor_config(const char* src){
	uint8_t offset = 0;
	const char* p = src;
	char buff[16] = {0};
	char* d = &buff[0];
	uint8_t count = 0;
	
	set_old_param();
		
	while( *p!='\0' ){
		if( *p==','){
			*d = '\0';		
			
			if( !is_num(buff) ) 
			{
				set_param_from_old();
				return 0;
			}
			
			if( 0==offset ){
				params[4] = (uint32_t)atoi(buff);
			}else if( 1==offset ){
				params[5] = (uint32_t)atoi(buff);
			}else if( 2==offset ){
				params[6] = (uint32_t)atoi(buff);
			}else if( 3==offset ){
				params[7] = (uint32_t)atoi(buff);
			}else if( 4==offset ){
				params[8] = (uint32_t)atoi(buff);
			}else if( 5==offset ){
				params[9] = (uint32_t)atoi(buff);
			}
			
			++offset;
			d = buff;
			++p;
			++count;
			continue;
		}
		*d = *p;
		++d;
		++p;
	}
	
	*d = '\0';	
	if( !is_num(buff) ) 
	{
		set_param_from_old();
		return 0;
	}
				
	if( 0==offset ){
		params[4] = (uint32_t)atoi(buff);
	}else if( 1==offset ){
		params[5] = (uint32_t)atoi(buff);
	}else if( 2==offset ){
		params[6] = (uint32_t)atoi(buff);
	}else if( 3==offset ){
		params[7] = (uint32_t)atoi(buff);
	}else if( 4==offset ){
		params[8] = (uint32_t)atoi(buff);
	}else if( 5==offset ){
		params[9] = (uint32_t)atoi(buff);
	}
	++count;
	
	if( count!=6 ){
		set_param_from_old();
		return 0;
	}
	
	return 1;
}

static uint8_t parse_rc_map_config(const char* src){
	uint8_t offset = 0;
	const char* p = src;
	char buff[16] = {0};
	char* d = buff;
	uint8_t count = 0;
	
	set_old_param();
	
	while( *p!='\0' ){
		if( *p==',' ){
			*d = '\0';
			
			if( !is_num(buff) ) 
			{
				set_param_from_old();
				return 0;
			}
			
			if( 0==offset ){
				params[12] = (uint32_t)atoi(buff);
			}else if( 1==offset ){
				params[13] = (uint32_t)atoi(buff);
			}else if( 2==offset ){
				params[14] = (uint32_t)atoi(buff);
			}else if( 3==offset ){
				params[15] = (uint32_t)atoi(buff);
			}
			
			++offset;
			d = buff;
			++p;
			++count;
			continue;
		}
		*d = *p;
		++d;
		++p;
	}
	
	*d = '\0';
	
	if( !is_num(buff) ) 
	{
		set_param_from_old();
		return 0;
	}
	
	if( 0==offset ){
		params[12] = (uint32_t)atoi(buff);
	}else if( 1==offset ){
		params[13] = (uint32_t)atoi(buff);
	}else if( 2==offset ){
		params[14] = (uint32_t)atoi(buff);
	}else if( 3==offset ){
		params[15] = (uint32_t)atoi(buff);
	}
	
	++count;
	
	if( count!=4 ){
		set_param_from_old();
		return 0;
	}
	return 1;
}

static void get_param_msg(void){
	
	//load_param(reset_params, params, MAX_PARAM_LEN);
	set_param();
	
	sprintf((char*)msg_string, " <<  Param Information  >>>\r\n");	
	usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
	
	sprintf((char*)msg_string, "\tUART2:%d\tUART3:%d\r\n", usart2_baudrate, usart3_baudrate);	
	usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
	
	sprintf((char*)msg_string, "\tCAM: %d, %d, %d, %d, %d, %d, %d\r\n", camera_config.open_close_level_in, camera_config.open_level, camera_config.record_level, camera_config.take_photo_level, \
																																			camera_config.open_time_ms, camera_config.record_time_ms, camera_config.take_photo_time_ms );	
	usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
	
	sprintf((char*)msg_string, "\tRCMAP: %d, %d, %d, %d\r\n", rc_map.camera_open_close_map, rc_map.camera_take_photo_map, rc_map.camera_record_map, rc_map.motor_map);	
	usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
	
	sprintf((char*)msg_string, "\tMOTOR: %d, %d, %d, %d, %d, %d\r\n <<<   Param End  >>>\r\n", motor_config.rc_min_pwm, motor_config.rc_trim_pwm, motor_config.rc_max_pwm, \
																																														 motor_config.rc_error, motor_config.open_pwm, motor_config.close_pwm);	
	usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
		
}

static void mavlink_request_data(void){
	mavlink_request_data_stream_t rdata;
	
	rdata.req_message_rate = 20;    //20hz 
	rdata.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS;  //RC
	rdata.start_stop = 1;
	rdata.target_component = 0x01;
	rdata.target_system = 0x01;
	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_POSITION;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_EXTRA1;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_EXTRA2;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	rdata.req_message_rate = 0;   
	rdata.req_stream_id = MAV_DATA_STREAM_EXTRA3;  
	rdata.start_stop = 0;	
	mavlink_msg_request_data_stream_send_struct(MAVLINK_COMM_3, &rdata);
	
}

static void camera_open(uint8_t on) {
	if(on){  //open
		if(camera_config.open_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
		} 
	}else{   //close
		if(camera_config.open_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
		} 
	} 
}

	
static void camera_record(uint8_t on) {
	if(on){  //open
		if(camera_config.record_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
		} 
	}else{   //close
		if(camera_config.record_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
		} 
	} 
}

static void camera_photo(uint8_t on) {
	if(on){  //open
		if(camera_config.take_photo_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
		} 
	}else{   //close
		if(camera_config.take_photo_level){
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
		}else{
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
		} 
	} 
}

static void rc_process(mavlink_rc_channels_t* rc){
	uint16_t rc_in;
	uint16_t rc_min;
	uint16_t rc_max;
	uint16_t rc_trim;
	uint16_t rc_e;
	uint16_t values[11];
	values[0] = rc->chan6_raw;
	values[1] = rc->chan7_raw;
	values[2] = rc->chan8_raw;
	values[3] = rc->chan9_raw;
	values[4] = rc->chan10_raw;
	values[5] = rc->chan11_raw;
	values[6] = rc->chan12_raw;
	values[7] = rc->chan13_raw;
	values[8] = rc->chan14_raw;
	values[9] = rc->chan15_raw;
	values[10] = rc->chan16_raw;
	
	rc_min = motor_config.rc_min_pwm;
	rc_max = motor_config.rc_max_pwm;
	rc_trim = motor_config.rc_trim_pwm;
	rc_e = motor_config.rc_error + rc_trim;
	
	if( rc_min==0 || rc_max==0 ||  rc_trim==0 || rc_max<=rc_min || rc_trim>=rc_max ) return;
	
	//camera open and close
	rc_in = values[rc_map.camera_open_close_map-6];
	if( camera_open_close_old_pwm!=0 && fabs((int16_t)(camera_open_close_old_pwm-rc_in))>=300 && (int16_t)(rc_in-rc_e)>=300){
		if( 0==camera_open_close_flag ){
			debug_string("Camera Open / Close\r\n");
			camera_open_close_flag = 1;
			camera_open_time = HAL_GetTick();
		}
	}
	camera_open_close_old_pwm = rc_in;
	
	//camera take photo
	rc_in = values[rc_map.camera_take_photo_map-6];
	if( camera_take_photo_old_pwm!=0 && fabs((int16_t)(camera_take_photo_old_pwm-rc_in))>=300 && (int16_t)(rc_in-rc_e)>=300){
		if( 0==camera_take_photo_flag ){
			debug_string("Take Photo\r\n");
			camera_take_photo_flag = 1;
			camera_take_photo_time = HAL_GetTick();
		}
	}
	camera_take_photo_old_pwm = rc_in;	
	
	//camera record
	rc_in = values[rc_map.camera_record_map-6];
	if( camera_record_old_pwm!=0 && fabs((int16_t)(camera_record_old_pwm-rc_in))>=300 && (int16_t)(rc_in-rc_e)>=300){
		if( 0==camera_record_flag ){
			debug_string("Camera Record\r\n");
			camera_record_flag = 1;
			camera_record_time = HAL_GetTick();
		}
	}
	camera_record_old_pwm = rc_in;	
	
	//motor close or open
	rc_in = values[rc_map.motor_map-6];
	if(motor_open_close_old_pwm!=0 && fabs(motor_open_close_old_pwm-rc_in)>=300){
		if( rc_in>rc_e ){  //open
			motor_open_close_flag = 1;
			debug_string("Motor Open\r\n");
		}else if( rc_in<rc_e ){ //close
			motor_open_close_flag = 2;
			debug_string("Motor Close\r\n");
		}
	}
	motor_open_close_old_pwm = rc_in;
	
	rc_last_update_time = HAL_GetTick();
	
	if( HAL_GetTick()-rc_send_callback_time>5000 ){
		sprintf((char*)msg_string, "From Pilot RC: CAM=%d, PH=%d, MOTOR=%d,\r\n", values[rc_map.camera_open_close_map-6], \
																																							values[rc_map.camera_take_photo_map-6],\
																																							values[rc_map.motor_map-6]);	
		usarts[1].writes(&usarts[1], msg_string, strlen((const char*)msg_string));
		rc_send_callback_time = HAL_GetTick();
	}
	
}

void mavlink_send(unsigned char* buff, uint32_t len){
	usarts[2].writes(&usarts[2], buff, len);
}

static void debug_string(const uint8_t* str){
	usarts[1].writes(&usarts[1], (uint8_t*)str, strlen((const char*)str));
}

static void set_old_param(void){
	uint8_t i = 0;
	
	for( i=0; i<MAX_PARAM_LEN; ++i ){
		old_params[i] = params[i];
	}
}

static void set_param_from_old(void){
	uint8_t i = 0;
	
	for( i=0; i<MAX_PARAM_LEN; ++i ){
		params[i] = old_params[i];
	}
}

static void reset_param(void){
	uint8_t i = 0;
	
	for( i=0; i<MAX_PARAM_LEN; ++i ){
		params[i] = reset_params[i];
	}
}

static void restart_usart_check(void){
	uint32_t update_time;
	
	update_time = usart2_update_time;
	if( HAL_GetTick()>update_time && HAL_GetTick()-update_time>60*1000){
		reconfig_usart(2);
		usart2_update_time= HAL_GetTick();
	}
	
	update_time = usart3_update_time;
	if( HAL_GetTick()>update_time && HAL_GetTick()-update_time>60*1000){
		reconfig_usart(3);
		usart3_update_time= HAL_GetTick();
	}
}

static void init_gpio_level(void){
	if( camera_config.open_level ){
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
	}else HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
	
	if( camera_config.take_photo_level ){
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
	}else {
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
	}	
}

static void update_motor_output(void){
	htim1.Instance->CCR2 = motor_config.close_pwm;
	htim1.Instance->CCR3 = motor_config.close_pwm;
}
